!********************************************************************
! Given arrays xs(:),ys(:) of n x and y values, this SUBROUTINE will
! fit a degree n-1 polynomial to the data, evaluate at x, and return
! the value as y and a (crude) error estimate as yerr. It uses
! Neville's algorithm (ref: sec 3-1 of Numerical Recipes in FORTRAN)
!********************************************************************
module mPolynomialInterpolation

        implicit none

        interface interpolate
                module procedure interpolate_general
        end interface interpolate

        interface extrapolate
                module procedure extrapolate_0
                module procedure extrapolate_0_array
                module procedure extrapolate_0_vector
        end interface extrapolate

        private
        public :: interpolate, extrapolate

contains

        pure subroutine interpolate_general(xs, ys, x, y, yerr)
                use mPrecision
                use mMisc
                real(D), intent(in) :: xs(:), ys( size(xs) )
                real(D), intent(in)  :: x
                real(D), intent(out) :: y, yerr
                integer :: iCurrent
                real(D) :: xDiffs( size(xs) )
                real(D) :: Cee( size(xs) ), Dee( size(xs) - 1 )
                integer :: m, n, n_m
                real(D) :: factor( size(xs) - 1 )

                n = size(xs)

                ! Set iCurrent to closest x:
                xDiffs = xs - x
                iCurrent = iminloc( abs(xDiffs) )

                ! Then the zeroth-order approximation is:
                y = ys(iCurrent)

                Cee = ys
                Dee = ys(1:n - 1)

                iCurrent = iCurrent - 1

                do m = 1, n - 1 ! For each column in the tableau... 
                        n_m = n - m
                        factor(1:n_m) = ( Cee(2:n_m + 1) - Dee(1:n_m) ) / ( xs(1:n_m) - xs(m + 1:n) )
                        Cee(1:n_m) = factor(1:n_m) * xDiffs(1:n_m)
                        Dee(1:n_m) = factor(1:n_m) * xDiffs(m + 1:n)
                        ! Pick which direction to go through the tableau:
                        ! This condition follows a straight line to the apex:
                        ! The result is that, if the coordinates in p are
                        ! ordered, the last dy will be the delta from adding
                        ! the furthest data point from x, which is probably
                        ! the most stable error estimate.
                        if(2 * iCurrent < n_m) then
                                yerr = Cee(iCurrent + 1)
                        else
                                yerr = Dee(iCurrent)
                                iCurrent = iCurrent - 1
                        end if

                        y = y + yerr
                end do

        end subroutine interpolate_general

        !*******************************************************************
        ! Special case for extrapolate to x = 0 and the values xs are
        ! decreasing
        !*******************************************************************
        pure subroutine extrapolate_0(xs, ys, y, yerr)
                use mPrecision
                real(D), intent(in) :: xs(:), ys( size(xs) )
                real(D), intent(out) :: y, yerr
                real(D) :: Cee( size(xs) ), Dee( size(xs) - 1 )
                integer :: m, n, n_m
                real(D) :: factor( size(xs) - 1 )

                n = size(xs)

                ! In this case the zeroth-order approximation is:
                y = ys( size(ys) )

                Cee = ys
                Dee = ys(1:n - 1)

                do m = 1, n - 1 ! For each column in the tableau... 

                        n_m = n - m

                        factor(1:n_m) = ( Cee(2:n_m + 1) - Dee(1:n_m) ) / ( xs(1:n_m) - xs(m + 1:n) )

                        Cee(1:n_m) = factor(1:n_m) * xs(1:n_m)
                        Dee(1:n_m) = factor(1:n_m) * xs(m + 1:n)

                        yerr = Dee(n_m)

                        y = y + yerr
                end do

        end subroutine extrapolate_0

        !*******************************************************************
        ! Special case for extrapolating a set of size(xs, 2) function
        ! evaluations to xs = 0. For a given function i, the first index of
        ! xs(:, i) indexes the sequence of evaluations of i, and must be
        ! decreasing, i.e., xs(1, i) > xs(2, i) > ... > xs( size(xs, 1), i )
        !*******************************************************************
        pure subroutine extrapolate_0_array(xs, ys, y, yerr)
                use mPrecision
                real(D), intent(in) :: xs(:, :), ys( size(xs, 1), size(xs, 2) )
                real(D), intent(out) :: y( size(xs, 2) ), yerr( size(xs, 2) )
                real(D) :: Cee( size(xs, 1), size(xs, 2) ), Dee( size(xs, 1) - 1, size(xs, 2) )
                integer :: m, n, n_m
                real(D) :: factor( size(xs, 1) - 1, size(xs, 2) )

                ! n is the order of the fit polynomial:
                n = size(xs, 1)

                ! In this case the zeroth-order approximation is:
                y = ys( size(ys, 1), : )

                if(n == 1) then
                        yerr = y
                else
                        Cee = ys
                        Dee = ys(1:n - 1, :)

                        do m = 1, n - 1 ! For each column in the tableau... 

                                n_m = n - m

                                factor(1:n_m, :) = ( Cee(2:n_m + 1, :) - Dee(1:n_m, :) ) / ( xs(1:n_m, :) - xs(m + 1:n, :) )

                                Cee(1:n_m, :) = factor(1:n_m, :) * xs(1:n_m, :)
                                Dee(1:n_m, :) = factor(1:n_m, :) * xs(m + 1:n, :)

                                yerr = Dee(n_m, :)
                                
                                y = y + yerr
                        end do
                end if

        end subroutine extrapolate_0_array

        !*******************************************************************
        ! Similar to extrapolate_0_array, but the evaluation points xs are
        ! the same for all size(ys, 2) functions.
        !*******************************************************************
!        pure subroutine extrapolate_0_vector(xs, ys, y, yerr)
        subroutine extrapolate_0_vector(xs, ys, y, yerr)
                use mPrecision
                use mMisc
!                real(D), intent(in) :: xs(:), ys( size(xs), : )
                real(D), intent(in) :: xs(:) 
!                real(D), intent(in) :: ys( :, : )
                real(D), intent(in) :: ys( :, : )
               real(D), intent(out) :: y( size(ys, 2) ), yerr( size(ys, 2) )
                real(D) :: Cee( size(xs), size(ys, 2) ), &
                           Dee( size(xs) - 1, size(ys, 2) )
                integer :: m, n, n_m
                real(D) :: factor( size(xs) - 1, size(ys, 2) )

                call assert( size(xs) == size(ys, 1), 'extrapolate_0_vector misused!' )

                ! n is the order of the fit polynomial:
!                n = size(ys, 1)
                n = size(xs)

                ! In this case the zeroth-order approximation is:
!                y = ys( size(ys, 1), : )
                y(:) = ys( size(xs), : )

                if(n == 1) then
                        yerr(:) = y(:) ! Just set error to 100%
                else
                        Cee(:, :) = ys(:, :)
                        Dee(:, :) = ys(1:n - 1, :)

                        do m = 1, n - 1 ! For each column in the tableau... 

                                n_m = n - m

                                factor(1:n_m, :) = ( Cee(2:n_m + 1, :) - Dee(1:n_m, :) ) / ( spread( xs(1:n_m) - xs(m + 1:n), 2, n ) )

                                Cee(1:n_m, :) = factor(1:n_m, :) * spread( xs(1:n_m), 2, n )
                                Dee(1:n_m, :) = factor(1:n_m, :) * spread( xs(m + 1:n), 2, n )

!                                yerr(:) = Dee(n_m, :)

!                                y(:) = y(:) + yerr(:)
                                y(:) = y(:) + Dee(n_m, :)
                        end do

                        yerr(:) = Dee(1, :)

                end if

        end subroutine extrapolate_0_vector

end module mPolynomialInterpolation
